# 在这里写上你的代码 :-)
# -*- coding:utf-8 -*-
# 导入相关模块
from machine import Pin,PWM,ADC
from NeoPID import PID
#import bluetooth,ble_uart_peripheral,utime

class TraCar:
    def __init__(self):
        self.PWMCar = PWMCarControl()
        self.adc_input=ADC_sensor
        pid=PID(0.7,0.2,0.1)
        #Pin(2, Pin.OUT).value(0)  # 初始化led指示灯。
        while True:
            adc_value=self.adc_input.read_value()
            avrg=(adc_value[1]+adc_value[2])/2
            if avrg < 1000:
                err=adc_value[1]-adc_value[2]
#             elif 1000 < avrg & avrg < 2000:
#                 err=adc_value[0]-adc_value[3]
            elif 2000 < avrg & avrg < 3000:
                find_line()
                adj=pid.get_adj_value(err)
            try:
                self.PWMCar.CarRun(adj)
            except: 
                pass
        
        
        
                
class PWMCarControl:
    def __init__(self):
        #LeftPin
        self.L1 = PWM(Pin(6), freq=1000, duty=0) # 初始化PWM 。这个PWM由32号引脚输出，频率为1000，初始占空比为0）。
        self.L2 = PWM(Pin(7), freq=1000, duty=0) # 初始化PWM 。这个PWM由33号引脚输出，频率为1000，初始占空比为0）。
        #RightPin
        self.R1 = PWM(Pin(8), freq=1000, duty=0)
        self.R2 = PWM(Pin(9), freq=1000, duty=0)
        
        self.speed = 1000 # 定义初始速度
        


    def CarRun(self,adj):
        if adj<0 :
            self.L1.duty(self.speed + adj)
            self.L2.duty(0)
            self.R1.duty(self.speed)
            self.R2.duty(0)
        elif adj>0 :
            self.L1.duty(self.speed)
            self.L2.duty(0)
            self.R1.duty(self.speed - adj)
            self.R2.duty(0)
            
        else:
            self.L1.duty(self.speed)
            self.L2.duty(0)
            self.R1.duty(self.speed)
            self.R2.duty(0)

class ADC_sensor_loop:
    def __init__(self,pin_list):
        self.adc=[]

        for i in pin_list:
            self.adc.append(ADC(Pin(i)))
        for i in self.adc:
            i.atten(ADC.ATTN_11DB)
            
    def read_value():
        a=[0,0,0,0] # 零点偏差
        for i in self.adc:
            n=self.adc.index(i)
            a[n]=i.read()+a[n]
        return a
    
class ADC_sensor:
    def __init__(self):
        self.in1 = ADC(Pin(2))
        self.in2 = ADC(Pin(3))
        self.in3 = ADC(Pin(5))
        self.in4 = ADC(Pin(4))
        self.in1.atten(ADC.ATTN_11DB)  # 这里配置测量量程为3.3V
        self.in2.atten(ADC.ATTN_11DB)
        self.in3.atten(ADC.ATTN_11DB)
        self.in4.atten(ADC.ATTN_11DB)
        self.wf1=Wave_filtering(500)
        self.wf2=Wave_filtering(500)
        self.wf3=Wave_filtering(500)
        self.wf4=Wave_filtering(500)
           
    def read_value(self):
        v1=self.in1.read()+0 # 消除零点偏差
        v2=self.in2.read()+0
        v3=self.in3.read()+0
        v4=self.in4.read()+0
        v1=self.wf1.filtering(v1)
        v2=self.wf2.filtering(v2)
        v3=self.wf3.filtering(v3)
        v4=self.wf4.filtering(v4)
        return (v1,v2,v3,v4)
    
class Wave_filtering():
    def __init__(self,k):
        self.a=[0,0,0]
        self.c=k
    def filtering(self,d):
        s=0
        for i in self.a:
            s = s + i
        g=s/3
        if d<g-self.c:
            o = self.a[0]-self.c
        elif d>g+self.c:
            o = self.a[0]+self.c
        else:
            o = d
        self.a[2]=self.a[1]
        self.a[1]=self.a[0]
        self.a[0]=o
        return o      

Car = TraCar()